#include "ygz/Settings.h"

namespace ygz {

    namespace setting {
        // IMU related
        // *10, *100 是王京同学想出来的
        /**
         * For EuRoc dataset, according to V1_01_easy/imu0/sensor.yaml
         * The params:
         * sigma_g: 1.6968e-4       rad / s / sqrt(Hz)
         * sigma_gw: 1.9393e-5      rad / s^2 / sqrt(Hz)
         * sigma_a: 2.0e-3          m / s^2 / sqrt(Hz)
         * sigma_aw: 3.0e-3         m / s^3 / sqrt(Hz)
         */
        double gyrBiasRw2 = 2.0e-5 * 2.0e-5 * 10;   // 陀螺仪的随机游走方差
        double accBiasRw2 = 5.0e-3 * 5.0e-3 * 10;   // 加速度计随机游走方差
        double gyrMeasError2 = 1.7e-4 * 1.7e-4 / 0.005 * 100;   // 陀螺仪的测量方差
        double accMeasError2 = 2.0e-3 * 2.0e-3 / 0.005 * 100;   // 加速度计测量方差

        double gravity = 9.810;

        size_t numPyramid = 4;
        float scalePyramid = 2;      // 金字塔每层至下一层的倍数
        float *scaleFactors = nullptr;     // 各层缩放因子
        float *invScaleFactors = nullptr;  // 倒数
        float *levelSigma2 = nullptr;      // 缩放倍数平方
        float *invLevelSigma2 = nullptr;   // 平方倒数

        size_t localWindowSize = 10;

        Eigen::Matrix3d Rbc = [] {
            Eigen::Matrix3d mat;
            mat << 0.0148655429818, -0.999880929698, 0.00414029679422,
                    0.999557249008, 0.0149672133247, 0.025715529948,
                    -0.0257744366974, 0.00375618835797, 0.999660727178;
            return mat;
        }();
        Eigen::Vector3d tbc(-0.0216401454975, -0.064676986768, 0.00981073058949);
        Sophus::SE3d TBC(Rbc, tbc);

        // 原始图像分辨率
        int imageWidth = 752;
        int imageHeight = 480;

        // 外框
        int boarder = 20;

        // ORB Matcher 的阈值
        int ORBMatcher_TH_HIGH = 100;
        int ORBMatcher_TH_LOW = 50;

        // ORB Extractor 阈值
        int initTHFAST = 20;  // 初始门限
        int minTHFAST = 5;   // 纹理较差时使用的门限
        int extractFeatures = 1000;     // 特征点数量

        float GridElementWidthInv;
        float GridElementHeightInv;

        bool trackerComputeMarginals = false;   // Tracker是否要计算marg

        int numBackendKeyframes = 5;
        double keyframeTimeGapInit = 0.5;      // 初始化时，两个关键帧之间的时间距离
        double keyframeTimeGapTracking = 0.5;  // 正常跟踪，两个关键帧之间的时间距离
        bool dilateDoubleCoarse = false;
        int sparsityFactor = 5;

        float minGradHistCut = 0.5;
        float minGradHistAdd = 7;
        float fixGradTH = -1;
        float gradDownweightPerLevel = 0.75;
        bool selectDirectionDistribution = true;
        int pixelSelectionUseFast = 0;


        const int staticPattern[10][40][2] = {
                {{0,  0},  {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100},
                        {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, // .
                        {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100},
                        {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100},
                        {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100},
                        {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100},
                        {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100},
                        {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}},

                {{0,  -1}, {-1,   0},    {0,    0},    {1,    0},    {0,    1},
                        {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, // +
                        {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100},
                        {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100},
                        {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100},
                        {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100},
                        {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100},
                        {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}},

                {{-1, -1}, {1,    1},    {0,    0},    {-1,   1},    {1,    -1},
                        {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, // x
                        {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100},
                        {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100},
                        {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100},
                        {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100},
                        {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100},
                        {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}},

                {{-1, -1}, {-1,   0},    {-1,   1},    {-1,   0},
                        {0,    0}, {0,    1},    {1,    -1},   {1,    0},
                        {1,    1},    {-100, -100}, // full-tight
                        {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100},
                        {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100},
                        {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100},
                        {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100},
                        {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100},
                        {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100},
                        {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100},
                        {-100, -100}, {-100, -100}},

                {{0,  -2}, {-1,   -1},   {1,    -1},   {-2,   0},
                        {0,    0}, {2,    0},    {-1,   1},    {1,    1},
                        {0,    2},    {-100, -100}, // full-spread-9
                        {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100},
                        {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100},
                        {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100},
                        {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100},
                        {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100},
                        {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100},
                        {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100},
                        {-100, -100}, {-100, -100}},

                {{0,  -2}, {-1,   -1},   {1,    -1},   {-2,   0},
                        {0,    0}, {2,    0},    {-1,   1},    {1,    1},
                        {0,    2},    {-2,   -2}, // full-spread-13
                        {-2,   2},    {2,    -2},   {2,    2},    {-100, -100},
                        {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100},
                        {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100},
                        {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100},
                        {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100},
                        {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100},
                        {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100},
                        {-100, -100}, {-100, -100}},

                {{-2, -2}, {-2,   -1},   {-2,   -0},   {-2,   1},    {-2,   2},
                        {-1,   -2},   {-1,   -1},   {-1,   -0},   {-1,   1},    {-1,   2}, // full-25
                        {-0,   -2},   {-0,   -1},   {-0,   -0},   {-0,   1},    {-0,   2},
                        {+2,   -2},   {+2,   -1},   {+2,   -0},   {+2,   1},    {+2,   2},
                        {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100},
                        {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100},
                        {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}},

                {{0,  -2}, {-1,   -1},   {1,    -1},   {-2,   0},
                        {0,    0}, {2,    0},    {-1,   1},    {1,    1},
                        {0,    2},    {-2,   -2}, // full-spread-21
                        {-2,   2},    {2,    -2},   {2,    2},    {-3,   -1},
                        {-3,   1},    {3,    -1},   {3,    1},    {1,    -3},
                        {-1,   -3},   {1,    3},    {-1,   3},    {-100, -100},
                        {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100},
                        {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100},
                        {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100},
                        {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100},
                        {-100, -100}, {-100, -100}},

                {{0,  -2}, {-1,   -1},   {1,    -1},   {-2,   0},
                        {0,    0}, {2,    0},    {-1,   1},    {0,    2},
                        {-100, -100}, {-100, -100}, // 8 for SSE efficiency
                        {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100},
                        {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100},
                        {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100},
                        {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100},
                        {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100},
                        {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100},
                        {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100},
                        {-100, -100}, {-100, -100}},

                {{-4, -4}, {-4,   -2},   {-4,   -0},   {-4,   2},
                        {-4,   4}, {-2,   -4},   {-2,   -2},   {-2,   -0},
                        {-2,   2},    {-2,   4}, // full-45-SPREAD
                        {-0,   -4},   {-0,   -2},   {-0,   -0},   {-0,   2},
                        {-0,   4},    {+2,   -4},   {+2,   -2},   {+2,   -0},
                        {+2,   2},    {+2,   4},    {+4,   -4},   {+4,   -2},
                        {+4,   -0},   {+4,   2},    {+4,   4},    {-200, -200},
                        {-200, -200}, {-200, -200}, {-200, -200}, {-200, -200},
                        {-200, -200}, {-200, -200}, {-200, -200}, {-200, -200},
                        {-200, -200}, {-200, -200}, {-200, -200}, {-200, -200},
                        {-200, -200}, {-200, -200}},
        };

//        const int  **patternP =  staticPattern[8];

        const int staticPatternNum[10] = {1, 5, 5, 9, 9, 13, 25, 21, 8, 25};

        const  int staticPatternPadding[10] = {1, 1, 1, 1, 2, 2, 2, 3, 2, 4};
        float outlierTH = 12 * 12;
        float outlierTHSumComponent = 50 * 50;
        float overallEnergyTHWeight = 1;
        float desiredImmatureDensity = 3000;

        float maxPixSearch = 0.027;
        float trace_stepsize = 1.0;
        float trace_minImprovementFactor = 2;

        float trace_GNThreshold = 0.1;
        float trace_extraSlackOnTH = 0.1;
        float trace_slackInterval = 1.5;

        int minTraceTestRadius = 2;
        int trace_GNIterations = 3;

        int lineLenThreshold = 20;
        double msld_sample_interval = 2.0;

        int min_feature_matches = 5;
        int line_match_number_weight = 10;


    void initSettings() {
        // compute the scale factors in each frame
        scaleFactors = new float[numPyramid];
        levelSigma2 = new float[numPyramid];
        scaleFactors[0] = 1.0f;
        levelSigma2[0] = 1.0f;
        for (size_t i = 1; i < numPyramid; i++) {
            scaleFactors[i] = scaleFactors[i - 1] * scalePyramid;
            levelSigma2[i] = scaleFactors[i] * scaleFactors[i];
        }

        invScaleFactors = new float[numPyramid];
        invLevelSigma2 = new float[numPyramid];
        for (size_t i = 0; i < numPyramid; i++) {
            invScaleFactors[i] = 1.0f / scaleFactors[i];
            invLevelSigma2[i] = 1.0f / levelSigma2[i];
        }

        GridElementWidthInv = static_cast<float> ( FRAME_GRID_COLS ) / static_cast<float> ( imageWidth );
        GridElementHeightInv = static_cast<float> ( FRAME_GRID_ROWS ) / static_cast<float> ( imageHeight );
    }

    void destroySettings() {
        delete[] scaleFactors;
        delete[] levelSigma2;
        delete[] invScaleFactors;
        delete[] invLevelSigma2;
    }

}
}
